package test.crane;

import java.math.*;

import ptolemy.actor.*;
import ptolemy.kernel.*;
import ptolemy.kernel.util.*;
import ptolemy.data.*;
import ptolemy.data.type.*;
import ptolemy.math.Utilities;

//////////////////////////////////////////////////////////////////////////
//// CheckAlfa
/**

@author Ivan Jeukens
@version v 0.1 07/02/2002
*/
public class Plant extends TypedAtomicActor {

    public Plant(CompositeEntity container, String name) throws
            IllegalActionException, NameDuplicationException {
        super(container, name);
        
        brake = new TypedIOPort(this, "brake", true, false);
        brake.setTypeEquals(BaseType.BOOLEAN);

        vc = new TypedIOPort(this, "vc", true, false);
        vc.setTypeEquals(BaseType.DOUBLE);

        fd = new TypedIOPort(this, "fd", true, false);
        fd.setTypeEquals(BaseType.DOUBLE);

        xc = new TypedIOPort(this, "xc", false, true);
        xc.setTypeEquals(BaseType.DOUBLE);
        
        xl = new TypedIOPort(this, "xl", false, true);
        xl.setTypeEquals(BaseType.DOUBLE);

        alfa = new TypedIOPort(this, "alfa", false, true);
        alfa.setTypeEquals(BaseType.DOUBLE);
    }

    public TypedIOPort brake;
    public TypedIOPort vc;
    public TypedIOPort fd;
    
    public TypedIOPort xc;
    public TypedIOPort xl;
    public TypedIOPort alfa;
    
    ///////////////////////////////////////////////////////////////////
    ////                         public methods                    ////
    
    public void pruneDependencies() {
        super.pruneDependencies();

        removeDependency(brake, xc);
        removeDependency(brake, xl);
        removeDependency(brake, alfa);

        removeDependency(vc, xc);
        removeDependency(vc, xl);
        removeDependency(vc, alfa);

        removeDependency(fd, xc);
        removeDependency(fd, xl);
        removeDependency(fd, alfa);
    }

    public void initialize() throws IllegalActionException  {
        super.initialize();
        //((Director)getDirector()).fireAt(this, 0.0);

        _state = new double[5];
        for(int i = 0;i < 5;i++) _state[i] = 0.0;

        _newvc = 0.0;
        _brake = false;
        _fd = 0.0;

        Director d = getDirector();
        _nextFire = STEP;
        d.fireAt(this, _nextFire);
    }

    public void fire() throws IllegalActionException {
        if(fd.hasToken(0)) {
            _fd = ((DoubleToken) fd.get(0)).doubleValue();
        }

        if(brake.hasToken(0)) {
            _brake = ((BooleanToken) brake.get(0)).booleanValue();
        }

        if(vc.hasToken(0)) {
            _newvc = ((DoubleToken) vc.get(0)).doubleValue();
        }

        Director d = getDirector();
        if(d.getCurrentTime() == _nextFire) {
            _rk4_diff();

            xc.broadcast(new DoubleToken(_state[1]));
            alfa.broadcast(new DoubleToken(_state[3]));
            xl.broadcast(new DoubleToken(_state[1] + R*_state[3]));

            _nextFire = _nextFire + STEP;
            _nextFire = Utilities.round(_nextFire, d.getTimeResolution());
            d.fireAt(this, _nextFire);
        }
    }

    ///////////////////////////////////////////////////////////////////
    ////                        private methods                    ////

    private void _rk4_diff() {
        double[] k1 = new double[5];
        double[] k2 = new double[5];
        double[] k3 = new double[5];
        double[] k4 = new double[5];

        int i;

        for(i = 0; i < 5; i++) k1[i] = _state[i];
        k1 = _diff(k1);
        for(i = 0; i < 5; i++) k1[i]= k1[i] * STEP;
        
        for(i = 0; i < 5; i++) k2[i] = _state[i] + (k1[i]/2);
        k2 = _diff(k2);
        for(i = 0; i < 5; i++) k2[i]= k2[i] * STEP;
        
        for(i = 0; i < 5; i++) k3[i]= _state[i]+ (k2[i]/2);
        k3 = _diff(k3);
        for(i = 0; i < 5; i++) k3[i] = k3[i] * STEP;
        
        for(i = 0; i < 5; i++) k4[i]= _state[i] + k3[i];
        k4 = _diff(k4);
        for(i = 0; i < 5; i++) k4[i] = k4[i] * STEP;
        
        for(i = 0; i < 5; i++) k1[i] = (k1[i]+ (2*k2[i]) + (2*k3[i])+ k4[i])/6;
        for(i = 0; i < 5; i++) _state[i] = _state[i] + k1[i];
    }

    private double [] _diff(double [] state) {
        double xdot, x, dxdot_dt, dx_dt;
        double alphadot, alpha, dalphadot_dt, dalpha_dt;
        double fc, dfc_dt;

        if(_brake) {
            xdot = 0.0;
        }
        else {
            xdot = state[0];
        }

        x = state[1];
        alphadot = state[2];
        alpha = state[3];
        fc = state[4];
 
        dxdot_dt = (fc+ G*ML*alpha - DC*xdot)/MC;
        dx_dt = xdot;
        dalphadot_dt = (-G*alpha + (-G*alpha*ML + DC*xdot -fc)/MC + 
            (-DL*xdot -R*DL*alphadot + _fd)/ML)/R;
        dalpha_dt = alphadot;
        dfc_dt = (4 * _newvc) - fc;

        state[0] = dxdot_dt;
        state[1] = dx_dt;
        state[2] = dalphadot_dt;
        state[3] = dalpha_dt;
        state[4] = dfc_dt;

        return state;
    }
    
    ///////////////////////////////////////////////////////////////////
    ////                      private variables                    ////

    private static final double STEP = 0.002;

    private double _nextFire = 0.0;
    private double [] _state;
    private double _newvc;
    private double _fd;
    private boolean _brake;
 
    private static final double MC = 10;
    private static final double ML = 100;
    private static final double G = 9.81;
    private static final double DC = 0.5;
    private static final double R = 5.0;
    private static final double DL = 0.01;
}
